use pyo3::prelude::*;
use crate::Vehicle;

#[pyclass]
pub struct VehicleController {
    vehicle: Vehicle,
}

#[pymethods]
impl VehicleController {
    #[new]
    pub fn new() -> PyResult<Self> {
        Ok(VehicleController { 
            vehicle: Vehicle::new()?
        })
    }

    pub fn red_light_on(&mut self) -> PyResult<()> {
        self.vehicle.led_light_on(0x00)
    }

    pub fn set_led_unit_on(&mut self, unit_id: u8, color: u8) -> PyResult<()> {
        self.vehicle.set_led_unit_on(unit_id, color)
    }

    pub fn set_led_unit_off(&mut self, unit_id: u8) -> PyResult<()> {
        self.vehicle.set_led_unit_off(unit_id)
    }

    pub fn servo_yaw(&mut self, angle: u8) -> PyResult<()> {
        self.vehicle.set_servo_angle(2, angle)
    }

    pub fn servo_pitch(&mut self, angle: u8) -> PyResult<()> {
        self.vehicle.set_servo_angle(1, angle)
    }

    pub fn forward(&mut self) -> PyResult<()> {
        for i in 0..4 {
            self.vehicle.start_motor(i)?;
        }
        Ok(())
    }

    pub fn backward(&mut self) -> PyResult<()> {
        for i in 0..4 {
            self.vehicle.reverse_motor(i)?;
        }
        Ok(())
    }

    pub fn stop(&mut self) -> PyResult<()> {
        for i in 0..4 {
            self.vehicle.stop_motor(i)?;
        }
        Ok(())
    }

    pub fn strafe_left(&mut self) -> PyResult<()> {
        self.vehicle.reverse_motor(0)?;
        self.vehicle.start_motor(1)?;
        self.vehicle.start_motor(2)?;
        self.vehicle.reverse_motor(3)?;
        Ok(())
    }

    pub fn strafe_right(&mut self) -> PyResult<()> {
        self.vehicle.start_motor(0)?;
        self.vehicle.reverse_motor(1)?;
        self.vehicle.reverse_motor(2)?;
        self.vehicle.start_motor(3)?;
        Ok(())
    }

    pub fn diagonal_front_left(&mut self) -> PyResult<()> {
        self.vehicle.stop_motor(0)?;
        self.vehicle.start_motor(1)?;
        self.vehicle.start_motor(2)?;
        self.vehicle.stop_motor(3)?;
        Ok(())
    }

    pub fn diagonal_back_left(&mut self) -> PyResult<()> {
        self.vehicle.reverse_motor(0)?;
        self.vehicle.stop_motor(1)?;
        self.vehicle.stop_motor(2)?;
        self.vehicle.reverse_motor(3)?;
        Ok(())
    }

    pub fn diagonal_front_right(&mut self) -> PyResult<()> {
        self.vehicle.start_motor(0)?;
        self.vehicle.stop_motor(1)?;
        self.vehicle.stop_motor(2)?;
        self.vehicle.start_motor(3)?;
        Ok(())
    }

    pub fn diagonal_back_right(&mut self) -> PyResult<()> {
        self.vehicle.stop_motor(0)?;
        self.vehicle.reverse_motor(1)?;
        self.vehicle.reverse_motor(2)?;
        self.vehicle.stop_motor(3)?;
        Ok(())
    }

    pub fn spin_left(&mut self) -> PyResult<()> {
        self.vehicle.reverse_motor(0)?;
        self.vehicle.reverse_motor(1)?;
        self.vehicle.start_motor(2)?;
        self.vehicle.start_motor(3)?;
        Ok(())
    }

    pub fn spin_right(&mut self) -> PyResult<()> {
        self.vehicle.start_motor(0)?;
        self.vehicle.start_motor(1)?;
        self.vehicle.reverse_motor(2)?;
        self.vehicle.reverse_motor(3)?;
        Ok(())
    }
}